import processing.video.*;
import FaceDetect.*;

FaceDetect fd;
//FaceDetect ed;
Capture cam;
PImage rotatedFrame;
PImage lastFace;
boolean isTracking;

// random constants
float faceWidener = 1.05;

// mask
PImage faceMask;

// IIR smoothing.
final float alpha = .35;
float lastFaceX;
float lastFaceY;
float lastScale;
float volatility;

// Face detection vars
int MAX = 10;
int[] x = new int[MAX];
int[] y = new int[MAX];
int[] r = new int[MAX];
int[][] Faces = new int[MAX][3];

// memoized trig caches
float[] thetaCache;
int cacheSlices = 1440;
float[] cosCache = new float[cacheSlices];
float[] sinCache = new float[cacheSlices];

void setup(){
  size(640,480,P3D);
  cam = new Capture(this, width, height);
  // detection seems to work with whatever haarcascade was loaded last
  //ed = new FaceDetect(this);
  //ed.start("haarcascade_mcs_eyepair_small.xml", width, height, 5);
  fd = new FaceDetect(this);
  fd.start("haarcascade_frontalface_alt.xml", height, width, 5);
  rotatedFrame = new PImage(height,width);
  
  lastFace = new PImage(width, height);

  // mask
  //PImage fmBase = loadImage("facemask-marked.jpg");
  PImage fmBase = loadImage("facemask.jpg");
  faceMask = loadImage("black.jpg");
  faceMask.mask(fmBase);
  
  // initialize trig caches
  thetaCache = new float[width * height];
  for (int i = 0; i < thetaCache.length; i++) {
    thetaCache[i] = -1;
  }
  for (int j = 0; j < cacheSlices; j++) {
    float theta = float(j)/float(cacheSlices) * TWO_PI;
    cosCache[j] = cos(theta);
    sinCache[j] = sin(theta);
  }
}

void draw() {
  if (cam.available()) {
    cam.read();

    // do detection on frame rotated 90 CW (from camera's POV)
    cam.loadPixels();
    rotatedFrame.loadPixels();
    for (int i = 0; i < cam.pixels.length; i++) {
      int row = floor(i/width);
      int col = i % width;
      rotatedFrame.pixels[(col*height)+height-row-1] = cam.pixels[i];
    }
    rotatedFrame.updatePixels();
    Faces = fd.face(rotatedFrame);

    int count = Faces.length;
    pushMatrix();
    if (count>0 && Faces[0][2] > 35) {
      isTracking = true;
      float faceX = (alpha*Faces[0][1]) + ((1-alpha)*lastFaceX);
      float faceY = (alpha*(height-Faces[0][0])) + ((1-alpha)*lastFaceY);
      float scale = (alpha*(width/float(Faces[0][2])/2.45)) + ((1-alpha)*lastScale);

      // deal with volatility
      volatility = abs(faceX-lastFaceX) + abs(faceY-lastFaceY);
      if (volatility > 3) {
        int posterizeLevel = int(12-volatility);
        if (posterizeLevel < 2) posterizeLevel = 2;
        cam.filter(POSTERIZE, posterizeLevel);
      }
      
      // store last face, if things are looking good
      if (volatility < .75) storeLastFace();
      
      lastFaceX = faceX;
      lastFaceY = faceY;
      lastScale = scale;
      scale(-scale, scale*faceWidener);
      translate(-faceX,-faceY);
      drawFace(cam, scale);
    } else if (isTracking) {
      isTracking = false;
    }
    
    if (!isTracking && lastFace != null) {
      lastFace.filter(POSTERIZE, 2);
      scale(-lastScale, lastScale);
      translate(-lastFaceX,-lastFaceY);
      drawFace(lastFace, lastScale*faceWidener);
    }
    popMatrix();
  }
  image(faceMask,0,0);
}

void storeLastFace() {
  cam.loadPixels();
  lastFace.loadPixels();
  arraycopy(cam.pixels,lastFace.pixels);
  lastFace.updatePixels();
}

void drawFace(PImage face, float scale) {
  /*
  cam.loadPixels();
  
  // set origin
  int xOrig = width/2;
  int yOrig = height/2;

  loadPixels();
  for( int y=0; y<height; y++ )
  {
    for( int x=0; x<width; x++ )
    {
      // translate coords
      int xTrans = x - xOrig;
      int yTrans = y - yOrig;
      
      // convert x,y to polar
      float radius = dist(xOrig, yOrig, x, y);
      radius = radius < 200 ? radius : 200 + pow((radius-200),0.8);
      int imgX = int( radius * memoTrig("cos", xTrans, yTrans) );
      int imgY = int( radius * memoTrig("sin", xTrans, yTrans) );
      
      pixels[ y*width + x ] = cam.pixels[ (imgY+yOrig)*cam.width + (imgX+xOrig) ];
    }
  }
  updatePixels();
  */
  image(face, -1 * width/scale/2.1, height/scale/2); // 2.1 gives slight left adjustment
}

// Optimized calculation of sin, cos, and atan for any point relative to origin
float memoTrig(String fn, int x, int y) {
  int key = (abs(y)*width) + abs(x);
  float theta = thetaCache[key];
  // Calculate theta, caching by absolute values
  if (x == 0) { // Handle division by zero
    theta = y > 0 ? HALF_PI : PI + HALF_PI;
  } else {  
    if (theta == -1) {
      theta = atan( float(abs(y))/float(abs(x)));
      thetaCache[key] = theta;    
    }
    // Modify theta depending on which quadrant it's in. Default is top right.
    if (x < 0 && y >= 0) theta = PI - theta; // top left
    if (x <= 0 && y < 0) theta = PI + theta; // bottom left
    if (x > 0 && y < 0) theta = TWO_PI - theta; // bottom right
  }  
  
  // Return the value of cos or sin, using lookup tables.
  float val = 0;
  if (fn == "cos") val = cosCache[floor((theta/TWO_PI)*cacheSlices)];
  if (fn == "sin") val = sinCache[floor((theta/TWO_PI)*cacheSlices)];
  return val;
}

